#include "Lowpass_Filter.h"
#include "FreeRTOS.h"
#include "cmsis_os.h"

#define _2PI       6.28318530718f

float y = 0;
float Lowpassfilter_sim(float x)
{
	float out = 0.5f*x + 0.5f*y;
	y = x;
	return out;
}

float Lowpassfilter(struct LOWPASS *lowpass, float x) {
  // 获取当前系统时刻，单位是tick
  TickType_t currentTicks = xTaskGetTickCount();
  
  // 计算时间间隔dt，单位为秒
  float dt = (float)(currentTicks - lowpass->Last_Timesamp) / 1000;
  
  // 如果时间间隔过小或无变化，直接返回输入值
  if (dt < 0.0f || dt == 0) {
      dt = 0.0015f; // 设置一个最小时间间隔避免错误
  } else if (dt > 0.005f) {
      lowpass->Last_y = x;
      lowpass->Last_Timesamp = currentTicks;
      return x;  // 如果时间间隔过大，跳过滤波处理
  }

  // 计算低通滤波器输出
  float alpha = lowpass->Tf / (lowpass->Tf + dt);
  float y = alpha * lowpass->Last_y + (1.0f - alpha) * x;
  
  // 更新上一次的输出和时间
  lowpass->Last_y = y;
  lowpass->Last_Timesamp = currentTicks;

  return y;
}
